
#include <BShape>
#include <BGridPlane>
#include "IConvexPlaneAlgorithm.h"
#include "IContinuousConvexPlaneCast.h"
#include "member_BNode.h"
#include "member_BBody.h"
#include "member_BShape.h"

using namespace BWE;

IConvexPlaneAlgorithm::IConvexPlaneAlgorithm()
{

}
IConvexPlaneAlgorithm::~IConvexPlaneAlgorithm()
{

}

void IConvexPlaneAlgorithm::caculate()
{
	if (dynamic_cast<BGridPlane*>(_shapeA))
	{
		this->swap();
	}

	BGridPlane* gridPlane = dynamic_cast<BGridPlane*>(_shapeB);
	if (!_shapeA || !gridPlane)
		return;

	const BVector& normal = gridPlane->normal();
	const BReal& constant = gridPlane->constant();

	BMatrix planeInConvex = _matrixA.inverse() * _matrixB;
	BMatrix convexInPlane= _matrixB.inverse() * _matrixA;

	BVector vertex = shape_member(_shapeA)->supportVertex(planeInConvex.mult3(-normal));
	BVector vertexInPlane = convexInPlane * vertex;
	BReal distance = (normal.dot(vertexInPlane) - constant);

	BVector pointB = vertexInPlane - normal * distance;

	if (distance < 0)
	{
		BVector normalB = _matrixB.mult3(normal);
		this->addContactPoint(pointB, normalB, distance);
	}

	this->refreshContactPoints();
}
void IConvexPlaneAlgorithm::timeImpact()
{
	if (dynamic_cast<BGridPlane*>(_shapeA))
	{
		this->swap();
	}

	BGridPlane* gridPlane = dynamic_cast<BGridPlane*>(_shapeB);
	if (!_shapeA || !gridPlane)
		return;

	const BMatrix& matrixA = node_member(_bodyA)->matrix;
	const BMatrix& matrixB = node_member(_bodyB)->matrix;

	const BMatrix& interpMatrixA = body_member(_bodyA)->predictedMatrix;
	const BMatrix& interpMatrixB = body_member(_bodyB)->predictedMatrix;

	CastResult result;
	IContinuousConvexPlaneCast ccc(_shapeA, gridPlane);
	if (ccc.calcTimeOfImpact(matrixA, interpMatrixA, matrixB, interpMatrixB, result))
	{
		BVector normalB = _matrixB.mult3(result.normal);
		this->addContactPoint(result.point, normalB, result.distance, result.fraction);
	}
}
